import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription

def generate_launch_description():
    bringup_dir = get_package_share_directory('agv_startup')
 
    launch_dir = os.path.join(bringup_dir, 'launch')

    robot = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'agv.launch.py')),
        launch_arguments={'carto_slam': 'false'}.items(),
    )
    description = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(os.path.join(launch_dir, 'robot_mode_description.launch.py')),
        launch_arguments={'carto_slam': 'false'}.items(),
    )
    # stereo_sync参数
    stereo_sync_params = {
        'approx_sync': True,
        'approx_sync_max_interval': 0.01,
        'queue_size': 30,
        'slop': 0.01,
        
        # 双目参数
        'stereo_to_depth': True,
        'depth_scale': 1.0,
        'max_depth': 10.0,
        'min_depth': 0.3,
        'stereo_algorithm': 1,
        'optimize_epipolar': True,
        'frame_id': 'camera_infra1_optical_frame',
        
        # SGBM参数优化
        'stereo_params': {
            'preFilterSize': 9,
            'preFilterCap': 31,
            'uniquenessRatio': 10,
            'speckleWindowSize': 200,
            'speckleRange': 2,
            'disp12MaxDiff': 0,
            'minDisparity': 0,
            'numDisparities': 256,
            'blockSize': 11,
            'P1': 8 * 3 * 11 * 11,
            'P2': 32 * 3 * 11 * 11,
            'mode': 1
        }
    }

    # rtabmap参数
    rtabmap_params = {
        'frame_id': 'base_footprint',
        'visual_odometry': True,
        'icp_odometry': False,
        'stereo': True,
        'subscribe_rgbd': True,
        'subscribe_stereo': False,
        'subscribe_odom_info': True,    # True:选用内部视觉里程计或ICP里程计，False：选用外部里程计
        'subscribe_scan': True,
        'approx_sync': True,
        'Reg/Strategy': '0',            # 选择 ICP 来改进使用激光扫描通过 ICP 发现的全局闭环。0=Visual, 1=ICP, 2=Visual+ICP
        'Rtabmap/TimeThr': '2000',
        'Rtabmap/MemoryThr': '2000',

        'Grid/RangeMin': '0.2',
        'Grid/Sensor': 'false',         # Gridmap is come from depth camera or not, default:false, come from lidar
        'Reg/Force3DoF': 'true',        # 强制 3DoF 注册：roll、pitch和z不会被估计
        'Optimizer/Slam2D': True,       # 强制视觉里程计仅在3DOF（x，y，theta）中跟踪车辆,增加地图的鲁棒性,配合参数：Reg/Force3DoF=true
        'RGBD/AngularUpdate': '0.01',
        'RGBD/LinearUpdate': '0.01',
        'RGBD/NeighborLinkRefining': 'true',
        'wait_for_transform': 0.5,
        'latch': False,
        'wait_imu_to_init': False
    }

    # 更新双目视觉里程计参数
    stereo_odom_params = {
        'frame_id': 'base_footprint',
        'odom_frame_id': 'odom',
        'approx_sync': True,
        'approx_sync_max_interval': 0.01,
        'queue_size': 30,
        'wait_for_transform': 0.5,
        
        # 双目匹配参数优化
        'Stereo/WinSize': '21',           # 增大窗口大小
        'Stereo/MaxDisparity': '256',     # 增大最大视差值
        'Stereo/MinDisparity': '0',       # 最小视差
        'Stereo/OpticalFlow': 'true',     # 使用光流
        'Stereo/MaxLevel': '5',           # 增加金字塔层数
        'Stereo/Iterations': '50',        # 增加迭代次数
        'Stereo/EpipolarGeometry': 'true', # 使用极线几何约束
        'Stereo/StereoOpticalFlow': 'true', # 启用立体光流
        'Stereo/MaxSlope': '0.7',         # 视差梯度限制
        
        # 特征提取和匹配参数优化
        'Vis/EstimationType': '0',         # 使用3D->3D估计
        'Vis/MinInliers': '15',            # 降低最小内点要求
        'Vis/InlierDistance': '0.15',      # 增大内点距离阈值
        'Vis/MaxDepth': '10.0',            # 最大深度
        'Vis/MinDepth': '0.3',             # 最小深度
        'Vis/MaxFeatures': '1500',         # 增加特征点数量
        'Vis/FeatureType': '8',            # 8=GFTT/BRIEF
        'Vis/MaxMeanError': '2.5',         # 增大平均误差容忍度
        'Vis/MaxError': '5.0',             # 增大最大误差容忍度
        'Vis/RoiRatios': '0.03 0.03 0.04 0.04',  # ROI裁剪比例
        'Vis/GridRows': '4',               # 网格行数
        'Vis/GridCols': '4',               # 网格列数
        
        # 2D约束
        'Reg/Force3DoF': 'true',           # 强制3DoF
        'Odom/Strategy': '0',              # Frame to Map
        'Odom/GuessMotion': 'true',        # 使用运动预测
        'Odom/FillInfoData': 'true',       # 填充协方差信息
        'Odom/ResetCountdown': '0',        # 不自动重置
        'Odom/KeyFrameThr': '0.4',         # 降低关键帧阈值
        'Odom/LinearUpdate': '0.1',        # 线性更新阈值
        'Odom/AngularUpdate': '0.1',       # 角度更新阈值
        
        # 发布设置
        'publish_tf': True,
        'publish_null_when_lost': False,
        'publish_odom_tf': True
    }

    remappings = [
        ('left/image_rect', '/camera/camera/infra1/image_rect_raw'),
        ('right/image_rect', '/camera/camera/infra2/image_rect_raw'),
        ('left/camera_info', '/camera/camera/infra1/camera_info'),
        ('right/camera_info', '/camera/camera/infra2/camera_info'),
        ('scan', '/scan'),
        ('imu', '/imu/data_raw'),
        ('rgbd_image', '/rgbd_image'),
        ('odom', '/stereo_odometry')
    ]

    # 合并所有节点
    nodes = [
        description,
        robot,
                # 双目视觉里程计节点
        Node(
            package='rtabmap_odom',
            executable='stereo_odometry',  # 使用stereo_odometry
            name='stereo_odometry',
            output='screen',
            parameters=[rtabmap_params],
            remappings=remappings
        ),
        Node(
            package='rtabmap_sync',
            executable='stereo_sync',
            name='stereo_sync',
            output='screen',
            parameters=[rtabmap_params],
            remappings=remappings
        ),

        # rtabmap节点
        Node(
            package='rtabmap_slam',
            executable='rtabmap',
            name='rtabmap',
            output='screen',
            parameters=[rtabmap_params],
            remappings=remappings,
            arguments=['-d']
        )
    ]

    return LaunchDescription(nodes)